#include <ros/ros.h>
#include <mavros_msgs/CommandHome.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "set_home_client");
    ros::NodeHandle nh;

    // 创建服务客户端
    ros::ServiceClient client = nh.serviceClient<mavros_msgs::CommandHome>("/mavros/cmd/set_home");

    // 定义服务请求和响应对象
    mavros_msgs::CommandHome srv;
    srv.request.current_gps = true;  // 使用当前 GPS 位置作为 Home

    // 调用服务
    if (client.call(srv)) {
        if (srv.response.success) {
            ROS_INFO("Home position set successfully!");
        } else {
            ROS_WARN("Failed to set home position. Result: %d", srv.response.result);
        }
    } else {
        ROS_ERROR("Failed to call service /mavros/cmd/set_home");
    }

    return 0;
}
